/*
**********************************************************************************
    OpenSURF642 : An TI DM642 implementation of OpenSURF.
    Copyright (C) 2011  Hai Yu

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
    
	This implementation is based on The original OpenSURF library created
	by Christopher Evans.The original work can be found at
		http://www.chrisevansdev.com/computer-vision-opensurf.html
	
    contact: yuhaihai12345@gmail.com	
**********************************************************************************
*/

#include "surf.h"
//! SURF priors (these need not be done at runtime)
const float pi = 3.14159f;

const double gauss25 [7][7] = {
  0.02350693969273,0.01849121369071,0.01239503121241,0.00708015417522,0.00344628101733,0.00142945847484,0.00050524879060,
  0.02169964028389,0.01706954162243,0.01144205592615,0.00653580605408,0.00318131834134,0.00131955648461,0.00046640341759,
  0.01706954162243,0.01342737701584,0.00900063997939,0.00514124713667,0.00250251364222,0.00103799989504,0.00036688592278,
  0.01144205592615,0.00900063997939,0.00603330940534,0.00344628101733,0.00167748505986,0.00069579213743,0.00024593098864,
  0.00653580605408,0.00514124713667,0.00344628101733,0.00196854695367,0.00095819467066,0.00039744277546,0.00014047800980,
  0.00318131834134,0.00250251364222,0.00167748505986,0.00095819467066,0.00046640341759,0.00019345616757,0.00006837798818,
  0.00131955648461,0.00103799989504,0.00069579213743,0.00039744277546,0.00019345616757,0.00008024231247,0.00002836202103
};

const double gauss33 [11][11] = {
  0.014614763,0.013958917,0.012162744,0.00966788,0.00701053,0.004637568,0.002798657,0.001540738,0.000773799,0.000354525,0.000148179,
  0.013958917,0.013332502,0.011616933,0.009234028,0.006695928,0.004429455,0.002673066,0.001471597,0.000739074,0.000338616,0.000141529,
  0.012162744,0.011616933,0.010122116,0.008045833,0.005834325,0.003859491,0.002329107,0.001282238,0.000643973,0.000295044,0.000123318,
  0.00966788,0.009234028,0.008045833,0.006395444,0.004637568,0.003067819,0.001851353,0.001019221,0.000511879,0.000234524,9.80224E-05,
  0.00701053,0.006695928,0.005834325,0.004637568,0.003362869,0.002224587,0.001342483,0.000739074,0.000371182,0.000170062,7.10796E-05,
  0.004637568,0.004429455,0.003859491,0.003067819,0.002224587,0.001471597,0.000888072,0.000488908,0.000245542,0.000112498,4.70202E-05,
  0.002798657,0.002673066,0.002329107,0.001851353,0.001342483,0.000888072,0.000535929,0.000295044,0.000148179,6.78899E-05,2.83755E-05,
  0.001540738,0.001471597,0.001282238,0.001019221,0.000739074,0.000488908,0.000295044,0.00016243,8.15765E-05,3.73753E-05,1.56215E-05,
  0.000773799,0.000739074,0.000643973,0.000511879,0.000371182,0.000245542,0.000148179,8.15765E-05,4.09698E-05,1.87708E-05,7.84553E-06,
  0.000354525,0.000338616,0.000295044,0.000234524,0.000170062,0.000112498,6.78899E-05,3.73753E-05,1.87708E-05,8.60008E-06,3.59452E-06,
  0.000148179,0.000141529,0.000123318,9.80224E-05,7.10796E-05,4.70202E-05,2.83755E-05,1.56215E-05,7.84553E-06,3.59452E-06,1.50238E-06
};

const short gauss25_short[7][7] = {
	770,605,406,231,112,46,16,
	711,559,374,214,104,43,15,
	559,439,294,168,81,34,12,
	374,294,197,112,54,22,8,
	214,168,112,64,31,13,4,
	104,81,54,31,15,6,2,
	43,34,22,13,6,2,0,
};
const short gauss33_short[11][11] = {
	478,457,398,316,229,151,91,50,25,11,4,
	457,436,380,302,219,145,87,48,24,11,4,
	398,380,331,263,191,126,76,42,21,9,4,
	316,302,263,209,151,100,60,33,16,7,3,
	229,219,191,151,110,72,43,24,12,5,2,
	151,145,126,100,72,48,29,16,8,3,1,
	91,87,76,60,43,29,17,9,4,2,0,
	50,48,42,33,24,16,9,5,2,1,0,
	25,24,21,16,12,8,4,2,1,0,0,
	11,11,9,7,5,3,2,1,0,0,0,
	4,4,4,3,2,1,0,0,0,0,0,
};

const short gauss_s1_short[81] = {
64,112,168,214,231,214,168,112,64,
112,197,294,374,406,374,294,197,112,
168,294,439,559,605,559,439,294,168,
214,374,559,711,770,711,559,374,214,
231,406,605,770,834,770,605,406,231,
214,374,559,711,770,711,559,374,214,
168,294,439,559,605,559,439,294,168,
112,197,294,374,406,374,294,197,112,
64,112,168,214,231,214,168,112,64,
};

const short gauss_s2_short[16] = {
952,1486,1486,952,
1486,2317,2317,1486,
1486,2317,2317,1486,
952,1486,1486,952,
};

const short sin_short[360] = {
0,571,1143,1714,2285,2855,3425,3993,4560,5125,5689,
6252,6812,7370,7927,8480,9031,9580,10125,10667,11206,
11742,12274,12803,13327,13847,14364,14875,15383,15885,16383,
16876,17363,17846,18323,18794,19259,19719,20173,20620,21062,
21497,21925,22347,22761,23169,23570,23964,24350,24729,25100,
25464,25820,26168,26509,26841,27165,27480,27787,28086,28377,
28658,28931,29195,29450,29696,29934,30162,30381,30590,30790,
30981,31163,31335,31497,31650,31793,31927,32050,32164,32269,
32363,32448,32522,32587,32642,32687,32722,32747,32762,32767,
32762,32747,32722,32687,32642,32587,32522,32448,32363,32269,
32164,32050,31927,31793,31650,31497,31335,31163,30981,30790,
30590,30381,30162,29934,29696,29450,29195,28931,28658,28377,
28086,27787,27480,27165,26841,26509,26168,25820,25464,25100,
24729,24350,23964,23570,23169,22761,22347,21925,21497,21062,
20620,20173,19719,19259,18794,18323,17846,17363,16876,16383,
15885,15383,14875,14364,13847,13327,12803,12274,11742,11206,
10667,10125,9580,9031,8480,7927,7370,6812,6252,5689,
5125,4560,3993,3425,2855,2285,1714,1143,571,0,
-571,-1143,-1714,-2285,-2855,-3425,-3993,-4560,-5125,-5689,
-6252,-6812,-7370,-7927,-8480,-9031,-9580,-10125,-10667,-11206,
-11742,-12274,-12803,-13327,-13847,-14364,-14875,-15383,-15885,-16383,
-16876,-17363,-17846,-18323,-18794,-19259,-19719,-20173,-20620,-21062,
-21497,-21925,-22347,-22761,-23169,-23570,-23964,-24350,-24729,-25100,
-25464,-25820,-26168,-26509,-26841,-27165,-27480,-27787,-28086,-28377,
-28658,-28931,-29195,-29450,-29696,-29934,-30162,-30381,-30590,-30790,
-30981,-31163,-31335,-31497,-31650,-31793,-31927,-32050,-32164,-32269,
-32363,-32448,-32522,-32587,-32642,-32687,-32722,-32747,-32762,-32767,
-32762,-32747,-32722,-32687,-32642,-32587,-32522,-32448,-32363,-32269,
-32164,-32050,-31927,-31793,-31650,-31497,-31335,-31163,-30981,-30790,
-30590,-30381,-30162,-29934,-29696,-29450,-29195,-28931,-28658,-28377,
-28086,-27787,-27480,-27165,-26841,-26509,-26168,-25820,-25464,-25100,
-24729,-24350,-23964,-23570,-23169,-22761,-22347,-21925,-21497,-21062,
-20620,-20173,-19719,-19259,-18794,-18323,-17846,-17363,-16876,-16383,
-15885,-15383,-14875,-14364,-13847,-13327,-12803,-12274,-11742,-11206,
-10667,-10125,-9580,-9031,-8480,-7927,-7370,-6812,-6252,-5689,
-5125,-4560,-3993,-3425,-2855,-2285,-1714,-1143,-571,};
const short cos_short[360] = {
32767,32762,32747,32722,32687,32642,32587,32522,32448,32363,32269,
32164,32050,31927,31793,31650,31497,31335,31163,30981,30790,
30590,30381,30162,29934,29696,29450,29195,28931,28658,28377,
28086,27787,27480,27165,26841,26509,26168,25820,25464,25100,
24729,24350,23964,23570,23169,22761,22347,21925,21497,21062,
20620,20173,19719,19259,18794,18323,17846,17363,16876,16383,
15885,15383,14875,14364,13847,13327,12803,12274,11742,11206,
10667,10125,9580,9031,8480,7927,7370,6812,6252,5689,
5125,4560,3993,3425,2855,2285,1714,1143,571,0,
-571,-1143,-1714,-2285,-2855,-3425,-3993,-4560,-5125,-5689,
-6252,-6812,-7370,-7927,-8480,-9031,-9580,-10125,-10667,-11206,
-11742,-12274,-12803,-13327,-13847,-14364,-14875,-15383,-15885,-16383,
-16876,-17363,-17846,-18323,-18794,-19259,-19719,-20173,-20620,-21062,
-21497,-21925,-22347,-22761,-23169,-23570,-23964,-24350,-24729,-25100,
-25464,-25820,-26168,-26509,-26841,-27165,-27480,-27787,-28086,-28377,
-28658,-28931,-29195,-29450,-29696,-29934,-30162,-30381,-30590,-30790,
-30981,-31163,-31335,-31497,-31650,-31793,-31927,-32050,-32164,-32269,
-32363,-32448,-32522,-32587,-32642,-32687,-32722,-32747,-32762,-32767,
-32762,-32747,-32722,-32687,-32642,-32587,-32522,-32448,-32363,-32269,
-32164,-32050,-31927,-31793,-31650,-31497,-31335,-31163,-30981,-30790,
-30590,-30381,-30162,-29934,-29696,-29450,-29195,-28931,-28658,-28377,
-28086,-27787,-27480,-27165,-26841,-26509,-26168,-25820,-25464,-25100,
-24729,-24350,-23964,-23570,-23169,-22761,-22347,-21925,-21497,-21062,
-20620,-20173,-19719,-19259,-18794,-18323,-17846,-17363,-16876,-16383,
-15885,-15383,-14875,-14364,-13847,-13327,-12803,-12274,-11742,-11206,
-10667,-10125,-9580,-9031,-8480,-7927,-7370,-6812,-6252,-5689,
-5125,-4560,-3993,-3425,-2855,-2285,-1714,-1143,-571,0,
571,1143,1714,2285,2855,3425,3993,4560,5125,5689,
6252,6812,7370,7927,8480,9031,9580,10125,10667,11206,
11742,12274,12803,13327,13847,14364,14875,15383,15885,16383,
16876,17363,17846,18323,18794,19259,19719,20173,20620,21062,
21497,21925,22347,22761,23169,23570,23964,24350,24729,25100,
25464,25820,26168,26509,26841,27165,27480,27787,28086,28377,
28658,28931,29195,29450,29696,29934,30162,30381,30590,30790,
30981,31163,31335,31497,31650,31793,31927,32050,32164,32269,
32363,32448,32522,32587,32642,32687,32722,32747,32762,};


const short atan_short[257] = {
0,0,
0,0,0,1,1,1,1,2,2,2,2,2,3,3,3,3,
4,4,4,4,4,5,5,5,5,6,6,6,6,6,7,7,
7,7,8,8,8,8,8,9,9,9,9,9,10,10,10,10,
11,11,11,11,11,12,12,12,12,12,13,13,13,13,14,14,
14,14,14,15,15,15,15,15,16,16,16,16,16,17,17,17,
17,17,18,18,18,18,18,19,19,19,19,19,20,20,20,20,
20,21,21,21,21,21,22,22,22,22,22,23,23,23,23,23,
24,24,24,24,24,24,25,25,25,25,25,26,26,26,26,26,
26,27,27,27,27,27,27,28,28,28,28,28,29,29,29,29,
29,29,30,30,30,30,30,30,31,31,31,31,31,31,32,32,
32,32,32,32,32,33,33,33,33,33,33,34,34,34,34,34,
34,34,35,35,35,35,35,35,36,36,36,36,36,36,36,37,
37,37,37,37,37,37,37,38,38,38,38,38,38,38,39,39,
39,39,39,39,39,40,40,40,40,40,40,40,40,41,41,41,
41,41,41,41,41,42,42,42,42,42,42,42,42,43,43,43,
43,43,43,43,43,43,44,44,44,44,44,44,44,44,45,};


static inline Int16 atan2_alternative(Int32 y, Int32 x)
{


	Int32 abs_y = _abs(y);
	Int32 abs_x = _abs(x);
	Int16 angle;

	if(x == 0 && y == 0)
		return 0;

	if(abs_y <= abs_x)
	{
		angle = atan_short[(abs_y<<8)/abs_x]; 
	}
	else
	{
		angle = atan_short[(abs_x<<8)/abs_y]; 
		angle = 90 - angle;
	}

	if(x < 0)
		angle = 180 - angle;

	if(y < 0)
		return 360 - angle;
	else return angle;
}



Surf * CreateSurf(IntegralImage * img, IPoint * ipts, Uint16 ipts_count)
{
	Surf * new_obj = (Surf *)malloc(sizeof(Surf));
	
	new_obj->ipts = ipts;
	new_obj->image = img;
	new_obj->index = 0;
	new_obj->ipts_count = ipts_count;


	return new_obj;

}


void DestroySurf(Surf * surf)
{
	free(surf);

}


void getDescriptors(Surf * self, Uint8 bUpright)
{

  int i;
  // Get the size of the vector for fixed loop bounds
  int ipts_size = self->ipts_count;
  // Check there are Ipoints to be described
  if (!self->ipts_count) return;
  if (bUpright)
  {
    // U-SURF loop just gets descriptors
    for (i = 0; i < ipts_size; ++i)
    {
      // Set the Ipoint to be described
      self->index = i;

      // Extract upright (i.e. not rotation invariant) descriptors
      getDescriptor(self,bUpright);
    }
  }
  else
  {
    // Main SURF-64 loop assigns orientations and gets descriptors
    for (i = 0; i < ipts_size; ++i)
    {
      // Set the Ipoint to be described
      self->index = i;

      // Assign Orientations and extract rotation invariant descriptors
      getOrientation(self);
      getDescriptor(self,0);
    }
  }


}

void getDescriptor(Surf * self, Uint8 bUpright)
{

  Int32 y, x, sample_x, sample_y, count=0;
  Int32 i = 0,j = 0;
  Int16 scale;
  Int16 *desc;
  Int32 dx, dy, mdx, mdy;
  Int16 co, si;
  Int16 gauss_s1 = 0, gauss_s2 = 0;
  Int32 rx = 0, ry = 0, rrx = 0, rry = 0, len = 0;
  Int32 s1_count, s2_count;  

  Int32 scale_co, scale_si;
  Int16 dx_s2,dy_s2, mdx_s2, mdy_s2;

  int k,l;
  IPoint *ipt = &self->ipts[self->index];
  scale = ipt->scale * 256;
  x = fRound(ipt->x);
  y = fRound(ipt->y);  
  desc = ipt->descriptor;

  if (bUpright)
  {
    co = 128;
    si = 0;
  }
  else
  {
	co = _sshvr(cos_short[ipt->orientation],8);
	si = _sshvr(sin_short[ipt->orientation],8);
  }

  scale_co = scale * co;
  scale_si = scale * si;

  i = -8;

  //Calculate descriptor for this interest point
  s2_count = 0;
  while(i < 12)
  {
    j = -8;
    i = i-4;

    while(j < 12) 
    {
      dx=dy=mdx=mdy=0;

      j = j - 4;

	  s1_count = 0;
      for (k = i; k < i + 9; ++k) 
      {
        for (l = j; l < j + 9; ++l, ++s1_count) 
        {
          //Get coords of sample point on the rotated axis
          sample_x = x + _sshvr((-l*scale_si + k*scale_co),15);
          sample_y = y + _sshvr((l*scale_co + k*scale_si),15);

          //Get the gaussian weighted x and y responses
          gauss_s1 = gauss_s1_short[s1_count];
          rx = haarX(self, sample_y, sample_x, scale/128);
          ry = haarY(self, sample_y, sample_x, scale/128);

          //Get the gaussian weighted x and y responses on rotated axis
          rrx = gauss_s1*((-rx*si + ry*co)/128)/(4096);
          rry = gauss_s1*((rx*co + ry*si)/128)/(4096);

          dx += rrx;
          dy += rry;
          mdx += _abs(rrx);
          mdy += _abs(rry);

        }
      }

      //Add the values to the descriptor vector
      gauss_s2 = gauss_s2_short[s2_count++];

	  dx_s2 = _sshvr(dx*gauss_s2, 15);
	  dy_s2 = _sshvr(dy*gauss_s2, 15);
	  mdx_s2 = _sshvr(mdx*gauss_s2,15);
	  mdy_s2 = _sshvr(mdy*gauss_s2,15);

      desc[count++] = dx_s2;
      desc[count++] = dy_s2;
      desc[count++] = mdx_s2;
      desc[count++] = mdy_s2;

      len += (dx_s2*dx_s2 + dy_s2*dy_s2 + mdx_s2*mdx_s2 + mdy_s2*mdy_s2) ;

      j += 9;
    }
    i += 9;
  }

  //Convert to Unit Vector
  len = sqrt(len);
  for(i = 0; i < 64; ++i)
    desc[i] *= 32768 / len;

}

float getAngle(float X, float Y)
{
  
  if(X > 0 && Y >= 0)
    return atan(Y/X);

  if(X < 0 && Y >= 0)
    return pi - atan(-Y/X);

  if(X < 0 && Y < 0)
    return pi + atan(Y/X);

  if(X > 0 && Y < 0)
    return 2*pi - atan(-Y/X);

  return 0;
  
  //return atan2f(Y,X);

}


void getOrientation(Surf * self)
{
  IPoint *ipt = &self->ipts[self->index];

  Int32 gauss = 0;
  float scale = ipt->scale;
  const Int16 s = fRound(scale), r = fRound(ipt->y), c = fRound(ipt->x);

  Int32 orientation_bin_x[ORIENTATION_BIN_SIZE] = {0}; 
  Int32 orientation_bin_y[ORIENTATION_BIN_SIZE] = {0}; 

  
  const Int16 id[] = {6,5,4,3,2,1,0,1,2,3,4,5,6};
  Int16 i,j;

  Int32 resX, resY;
  Int16 angle, angle_begin, angle_end;

  Int16 current_angle;
  

  Int16 orientation = 0;
  Int32 max = 0;

  Int32 length;
  Int32 index_temp;
  
  // calculate haar responses for points within radius of 6*scale
  for(i = -6; i <= 6; ++i) 
  {
    for(j = -6; j <= 6; ++j) 
    {
      if(i*i + j*j < 36) 
      {
        
		gauss = gauss25_short[id[i+6]][id[j+6]];
        resX = (gauss * haarX(self, r+j*s, c+i*s, 4*s))/16384;
        resY = (gauss * haarY(self, r+j*s, c+i*s, 4*s))/16384;

		angle = atan2_alternative(resY, resX);
		
		angle_begin = (angle - ORIENTATION_WINDOW_WIDTH/2);
		angle_end   = (angle + ORIENTATION_WINDOW_WIDTH/2);

		

		for(current_angle = angle_begin; current_angle <= angle_end; current_angle += ORIENTATION_SEARCH_RESOLUTION)
		{
			index_temp = current_angle;
			if(index_temp < 0)
				index_temp += 360;
			else if(index_temp >= 360)
				index_temp -= 360;
			orientation_bin_x[index_temp/ORIENTATION_SEARCH_RESOLUTION] += resX;
			orientation_bin_y[index_temp/ORIENTATION_SEARCH_RESOLUTION] += resY;
		}
        
      }
    }
  }

 
  for(i = 0; i < ORIENTATION_BIN_SIZE; i++)
  {
	   length = orientation_bin_x[i]*orientation_bin_x[i] + orientation_bin_y[i]*orientation_bin_y[i];

	   if(length > max)
	   {
			max = length;
			orientation = atan2_alternative(orientation_bin_y[i],orientation_bin_x[i]);
	   }
  }
  // assign orientation of the dominant response vector
  ipt->orientation = orientation;
}
